#ifndef RANSAC_GROUND_FILTER_HPP_
#define RANSAC_GROUND_FILTER_HPP_

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseArray.h>
#include <std_msgs/Header.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <Eigen/Dense>
#include <memory>
#include <string>
#include <vector>

namespace autoware_ground_segmentation
{

struct PlaneBasis
{
  Eigen::Vector3d e_x;
  Eigen::Vector3d e_y;
  Eigen::Vector3d e_z;
};

struct RGB
{
  double r = 0.0;
  double g = 0.0;
  double b = 0.0;
};

class RANSACGroundFilter
{
  using PointType = pcl::PointXYZ;

public:
  RANSACGroundFilter(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
  ~RANSACGroundFilter() = default;

private:
  void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& input_msg);
  
  void extractPointsIndices(
    const pcl::PointCloud<PointType>::Ptr in_cloud_ptr, 
    const pcl::PointIndices & in_indices,
    pcl::PointCloud<PointType>::Ptr out_only_indices_cloud_ptr,
    pcl::PointCloud<PointType>::Ptr out_removed_indices_cloud_ptr);

  Eigen::Affine3d getPlaneAffine(
    const pcl::PointCloud<PointType> segment_ground_cloud, 
    const Eigen::Vector3d & plane_normal);

  void applyRANSAC(
    const pcl::PointCloud<PointType>::Ptr & input, 
    pcl::PointIndices::Ptr & output_inliers,
    pcl::ModelCoefficients::Ptr & output_coefficients);

  void publishDebugMessage(
    const geometry_msgs::PoseArray & debug_pose_array,
    const pcl::PointCloud<PointType> & ground_cloud, 
    const std_msgs::Header & header);

  // ROS related
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;
  ros::Subscriber pointcloud_sub_;
  ros::Publisher ground_cloud_pub_;
  ros::Publisher no_ground_cloud_pub_;
  ros::Publisher debug_pose_array_pub_;
  ros::Publisher debug_ground_cloud_pub_;

  // Parameters
  std::string unit_axis_;
  int max_iterations_;
  int min_inliers_;
  int min_points_;
  double outlier_threshold_;
  double plane_slope_threshold_;
  double height_threshold_;
  double voxel_size_x_;
  double voxel_size_y_;
  double voxel_size_z_;
  bool debug_;
  bool is_initialized_debug_message_;
  Eigen::Vector3d unit_vec_;

  // Point cloud processing
  pcl::VoxelGrid<PointType> voxel_grid_;
  pcl::SACSegmentation<PointType> seg_;
  pcl::ExtractIndices<PointType> extract_indices_;
};

} // namespace autoware_ground_segmentation

#endif // RANSAC_GROUND_FILTER_HPP_ 